#include <Servo.h> 
#include <LiquidCrystal.h>
#define REC_CLOV A0
#define REC_HELIX A1
#define BETRIEB 13
#define SWITCH 10

//software version 1.5
//including a search routine
unsigned int pos_h = 90;
unsigned int pos_v = 140;

int helix;
int clov;
int clov_high;
int clov_low;
int helix_high;
int helix_low;
int v_neu;
int h_neu;
int x; // schleifenzaehler
int y;
int calibrated;
int prozent_clov;
int prozent_helix;
int lastmove = 0;
int latest_hit;
int intervall = 2000;
Servo v_servo;
Servo h_servo;
int timer_counter=0;

// ( RS, EN, D4,D5,D6,D7,RW-Masse )
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);

void setup()
{
  //Serial.begin(9600);  
  h_servo.attach(0);
  v_servo.attach(1);
  pinMode(BETRIEB, OUTPUT);
  pinMode(SWITCH, OUTPUT);
  pinMode(REC_CLOV, INPUT);
  pinMode(REC_HELIX, INPUT);
  pinMode(1, OUTPUT);
  pinMode(0, OUTPUT);
  h_servo.write(90);
  v_servo.write(150);
}


void calibrate ()
{
  //---------------------------------------------------------
  //calibrating
  //---------------------------------------------------------
  //clov_max = oberer map wert
  //clov_low = unterer map wert
  //we measure 0 first ! make sure sender is turned off !
  //10 seconds to turn it off !
  lcd.clear();
  lcd.print("Low");
  for ( x = 1 ; x<10; x++)
  {
    delay(1000);
  }
  clov_low = analogRead(REC_CLOV);
  delay (300);
  lcd.clear();
  lcd.print("high");
  helix_low = analogRead(REC_HELIX);
  // 30 seconds time to turn on the transmitter !
  for ( x = 1 ; x<20; x++)
  {
    delay(1000);
  }
  x=0;
  //----------------------------------------------------------
  clov_high = analogRead(REC_CLOV);
  helix_high = analogRead(REC_HELIX);
  calibrated = 1;
}

int read_antenna (int antenna)
 {
   //antenna 0 = Helix
   //antenna 1 = clover
  if ( antenna == 0 )
    {antenna = analogRead(REC_HELIX);
      int prozent = map (antenna, helix_low, helix_high, 1, 100);
      return prozent;}
  if (antenna == 1)
        {antenna = analogRead(REC_CLOV); 
        int prozent = map(antenna, clov_low, clov_high, 1, 100);
        prozent -= 3;
        //who cares about 3% lets call it tolerance ;)
        return prozent;}
 }


void moveservo()
  {
   while ( read_antenna(0) < read_antenna(1) )
     {
   delay(200);
   if ( pos_h >= 30 ) pos_h = pos_h - 1;
   if ( pos_h <= 30 && pos_v > 139 ) {pos_h = 170;pos_v = 115;}
   if ( pos_h <= 30 && pos_v > 114 ) {pos_h = 170;pos_v = 140;}
   h_servo.write(pos_h);
   v_servo.write(pos_v);
     } 
}

int better()
  {
    // the idea : move in a circle and remember the best value
    //-4/-4  0/-4  +4/-4
    //-4/0         +4/0
    //-4/+4  0/+4  +4/+4
  int max_v = 170;
  int min_v = 140;
  int max_h = 170;
  int min_h = 20;
  
  int startwert = read_antenna(0);
  int winkel = startwert / 10;
  int negativ_winkel = winkel * -1; 
 
  int mov_h [] = { negativ_winkel, negativ_winkel, 0, winkel, winkel ,winkel, 0, negativ_winkel, 0 };
  int mov_v [] = { 0, negativ_winkel, negativ_winkel, negativ_winkel, 0 ,winkel, winkel, winkel, 0 };
  
    // now move in a circle
    // get the original value of the antennaS
   
  int highest_value = 9; // set center as a start point
  int skip = 0; // if we are out of the range set skip flag
  for ( x=0; x < 8 ; x++)
   {
   int pos_h_temp = pos_h + mov_h[x];
   int pos_v_temp = pos_v + mov_v[x];
   if ( pos_h_temp > max_h or pos_h_temp < min_h ) skip=1;
   if ( pos_v_temp > max_v or pos_v_temp < min_v ) skip=1;
   h_servo.write(pos_h_temp);
   v_servo.write(pos_v_temp); 
   int temp_antenna = read_antenna(0);
   if  ( temp_antenna > startwert && skip == 0) highest_value=x;
   skip = 0;
   delay(300);
    }
   // best value shoud be in the array
   if ( latest_hit == highest_value )
       { intervall += 1; 
       latest_hit = highest_value;}
   else { intervall = 1; latest_hit = highest_value;}    
       
       
   pos_h = pos_h + mov_h[highest_value];
   pos_v = pos_v + mov_v[highest_value];
   h_servo.write(pos_h);
   v_servo.write(pos_v);
  }
      
void loop ()
{
  
  if ( calibrated != 1) calibrate();
  x=0; // schleifen auf 0 !
  y=0; // schleifen auf 0 ! 
  //pruefen was die werte machen
  helix = read_antenna(0);
  clov = read_antenna(1);
  
  // roland idee :) wir versuchen den wert zu verbessern indem wir link und recht pendeln :)
  timer_counter=0; // counter reset
  while  (helix > clov or helix == clov ) 
  {
    lcd.clear();
    lcd.print("He: ");
    lcd.print(helix);
    digitalWrite(SWITCH,HIGH); // sicher stellen dass unser 
    // if timer counter is 6, value is reached ( 2s ).
    // if we have 4 hits in a row, change to 10s
    // delay time for better routine 5 * intervall;
    int delay_time = 8 * intervall;
    if ( timer_counter == delay_time )
    { 
    better();
    timer_counter = 0;}
    helix = read_antenna(0);
    clov = read_antenna(1);
    delay(300);
    timer_counter++;
  }
  
  
  // shit something went wrong and we lost the plane......
 // ok lets check again ....

 delay (2000);
if (  read_antenna(0) <  read_antenna(1) ){
 intervall = 1;
 lcd.clear(); 
 lcd.print("CL: ");
 lcd.print(clov);  
 digitalWrite(SWITCH, LOW);
 moveservo();}
     
    
}






